#include "Loader.h"
 namespace ov_eval
 {

void Loader::load_data(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
                       std::vector<Eigen::Matrix3d> &cov_ori, std::vector<Eigen::Matrix3d> &cov_pos) {

  // Try to open our trajectory file
  std::ifstream file(path_traj);
  if (!file.is_open()) {
    PRINT_ERROR(RED "[LOAD]: Unable to open trajectory file...\n" RESET);
    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
    std::exit(EXIT_FAILURE);
  }

  // Loop through each line of this file
  std::string current_line;
  while (std::getline(file, current_line)) {

    // Skip if we start with a comment
    if (!current_line.find("#"))
      continue;

    // Loop variables
    int i = 0;
    std::istringstream s(current_line);
    std::string field;
    Eigen::Matrix<double, 20, 1> data;

    // Loop through this line (timestamp(s) tx ty tz qx qy qz qw Pr11 Pr12 Pr13 Pr22 Pr23 Pr33 Pt11 Pt12 Pt13 Pt22 Pt23 Pt33)
    while (std::getline(s, field, ' ')) {
      // Skip if empty
      if (field.empty() || i >= data.rows())
        continue;
      // save the data to our vector
      data(i) = std::atof(field.c_str());
      i++;
    }

    // Only a valid line if we have all the parameters
    if (i >= 20) {
      // time and pose
      times.push_back(data(0));
      poses.push_back(data.block(1, 0, 7, 1));
      // covariance values
      Eigen::Matrix3d c_ori, c_pos;
      c_ori << data(8), data(9), data(10), data(9), data(11), data(12), data(10), data(12), data(13);
      c_pos << data(14), data(15), data(16), data(15), data(17), data(18), data(16), data(18), data(19);
      c_ori = 0.5 * (c_ori + c_ori.transpose());
      c_pos = 0.5 * (c_pos + c_pos.transpose());
      cov_ori.push_back(c_ori);
      cov_pos.push_back(c_pos);
    } else if (i >= 8) {
      times.push_back(data(0));
      poses.push_back(data.block(1, 0, 7, 1));
    }
  }

  // Finally close the file
  file.close();

  // Error if we don't have any data
  if (times.empty()) {
    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
    std::exit(EXIT_FAILURE);
  }

  // Assert that they are all equal
  if (times.size() != poses.size()) {
    PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
    std::exit(EXIT_FAILURE);
  }

  // Assert that they are all equal
  if (!cov_ori.empty() && (times.size() != cov_ori.size() || times.size() != cov_pos.size())) {
    PRINT_ERROR(RED "[LOAD]: Parsing error, timestamps covariance size do not match!!\n" RESET);
    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
    std::exit(EXIT_FAILURE);
  }

  // Debug print amount
  // std::string base_filename = path_traj.substr(path_traj.find_last_of("/\\") + 1);
  // PRINT_DEBUG("[LOAD]: loaded %d poses from %s\n",(int)poses.size(),base_filename.c_str());
}

void Loader::load_data_csv(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
                           std::vector<Eigen::Matrix3d> &cov_ori, std::vector<Eigen::Matrix3d> &cov_pos) {

  // Try to open our trajectory file
  std::ifstream file(path_traj);
  if (!file.is_open()) {
    PRINT_ERROR(RED "[LOAD]: Unable to open trajectory file...\n" RESET);
    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
    std::exit(EXIT_FAILURE);
  }

  // Loop through each line of this file
  std::string current_line;
  while (std::getline(file, current_line)) {

    // Skip if we start with a comment
    if (!current_line.find("#"))
      continue;

    // Loop variables
    int i = 0;
    std::istringstream s(current_line);
    std::string field;
    Eigen::Matrix<double, 20, 1> data;

    // Loop through this line (groundtruth state [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel])
    while (std::getline(s, field, ',')) {
      // Skip if empty
      if (field.empty() || i >= data.rows())
        continue;
      // save the data to our vector
      data(i) = std::atof(field.c_str());
      i++;
    }

    // Only a valid line if we have all the parameters
    // Times are in nanoseconds -> convert to seconds
    // Our "fixed" state vector from the ETH GT format [q,p,v,bg,ba]
    if (i >= 8) {
      times.push_back(1e-9 * data(0));
      Eigen::Matrix<double, 7, 1> imustate;
      imustate(0, 0) = data(1, 0); // pos
      imustate(1, 0) = data(2, 0);
      imustate(2, 0) = data(3, 0);
      imustate(3, 0) = data(5, 0); // quat (xyzw)
      imustate(4, 0) = data(6, 0);
      imustate(5, 0) = data(7, 0);
      imustate(6, 0) = data(4, 0);
      poses.push_back(imustate);
    }
  }

  // Finally close the file
  file.close();

  // Error if we don't have any data
  if (times.empty()) {
    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
    std::exit(EXIT_FAILURE);
  }

  // Assert that they are all equal
  if (times.size() != poses.size()) {
    PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
    std::exit(EXIT_FAILURE);
  }
}

double Loader::get_total_length(const std::vector<Eigen::Matrix<double, 7, 1>> &poses) {

  // Loop through every pose and append its segment
  double distance = 0.0;
  for (size_t i = 1; i < poses.size(); i++) {
    distance += (poses[i].block(0, 0, 3, 1) - poses[i - 1].block(0, 0, 3, 1)).norm();
  }

  // return the distance
  return distance;
}
 }